#pragma once
#include "basic.h"

#pragma pack(push, 16)
class CEarth
{
private:
    double Re, f, e2, wie, g0, G0;
public:
    double RMh, RNh, clRNh, f_RMh, f_clRNh;
    CVec3 wnie, wnen, wnin, wnien, gn, gcc;
    CVec3 Mpvv;
    CMat3 Mpv;

    explicit CEarth(const CVec3 &pos = O31);
    ~CEarth() = default;

    void  init(const CVec3 &pos, int Method = 1, const CVec3 &vn = O31);
    void  update(const CVec3 &pos, int Method = 1, const CVec3 &vn = O31);
    CVec3 vn2dpos(const CVec3 &vn, double dt) const;
    void  clear();
};

class CSINS
{
private:
    double tk_1, dt, coeff;
    CVec3  wm, vm, wm1, vm1, phim, dvbm;
    CVec3  coningErr, rotationErr, scullingErr;

public:
    // EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
    double tk;
    CEarth eth;
    CVec3  wbib, wbeb, fn, fb, an;
    CVec3  att, vn, pos, eb, db;
    CVec3  vnL, posL;
    CMat3  Cnb;
    CQuat  qnb;

    explicit CSINS(const CVec3 &att = O31, const CVec3 &vn = O31, const CVec3 &pos = O31);
    ~CSINS() = default;

    void init(const CVec3 &att, const CVec3 &vn, const CVec3 &pos, double tk);
    void update(const double *gyr, const double *acc, double dt, int nSamples = NSAMPLES); 
    void outerLever(const CVec3 &lvr);
    void clear();
};
#pragma pack(pop)

#pragma pack(push, 8)
class CAvptRingBuffer
{
private:
    int head, cnt, size;
    double tk[AVPT_RING_BUFFER_LENGTH];     // s
    CVec3 att[AVPT_RING_BUFFER_LENGTH];
    CVec3 vel[AVPT_RING_BUFFER_LENGTH];
    CVec3 pos[AVPT_RING_BUFFER_LENGTH];
    
public:
    CVec3 atti, vni, posi;
    explicit CAvptRingBuffer(void);
    ~CAvptRingBuffer() = default;
    
    void push(double tk, const CVec3 &att, const CVec3 &vn = O31, const CVec3 &pos = O31);
    bool interp(double tpast);
    void clear();
};
#pragma pack(pop)